/*
* Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#include "log.h"
#include "RosManager.h"
#include <ros/ros.h>
#include <unistd.h>
#include <thread>

using namespace OHOS;
using namespace OHMW;

RosManager::RosManager()
{
    ros::init(ros::M_string(), "ohmw_ros1");
    std::thread(&RosManager::MasterDaemonThread, this).detach();
    std::thread(&RosManager::RosSpinThread, this).detach();
}

RosManager::~RosManager()
{
}

void RosManager::RosSpinThread()
{
    while (true)
    {
        if (ros::master::check())
        {
            OHMW_LOGI("spin in");
            ros::spin();
            OHMW_LOGI("spin out");
            exit(0);
        }
        else
        {
            usleep(1000000);
        }
    }
}
void RosManager::MasterDaemonThread()
{
    while (true)
    {
        if (ros::master::check())
        { // roscore正在运行
            if (!masterSwitchOn_)
            { // 关闭roscore
                OHMW_LOGI("roscore is running, stop it");
                std::system("pkill -f roscore");
            }
        }
        else
        { // roscore没有运行
            if (masterSwitchOn_)
            { // 启动roscore
                OHMW_LOGI("roscore is not running, start it");
                std::system("roscore > /dev/null &");
            }
        }
        usleep(1000000);
    }
}

void RosManager::Start() { masterSwitchOn_ = true; };
void RosManager::Stop()
{
    masterSwitchOn_ = false;
    nodes_.clear();
};

bool RosManager::CreateNode(std::string &path)
{
    if (nodes_.find(path) != nodes_.end())
    {
        OHMW_LOGW("node %s is already running", path.c_str());
        return false;
    }
    if (!ros::master::check())
    {
        OHMW_LOGW("roscore is not running");
        return false;
    }
    nodes_[path] = std::make_shared<RosNode>(path);
    OHMW_LOGI("rosnode %s is created", path.c_str());
    return true;
}

std::shared_ptr<RosNode> RosManager::GetNode(std::string &path)
{
    if (nodes_.find(path) == nodes_.end())
    {
        OHMW_LOGW("node %s is not running", path.c_str());
        return nullptr;
    }
    return nodes_[path];
}